//leetcode https://leetcode-cn.com/problems/minimum-cost-to-move-chips-to-the-same-position/
//https://blog.csdn.net/Haley__xu/article/details/108660393 service demo
//https://www.ncnynl.com/archives/201701/1283.html  service demo lei
#include <iostream>
#include <iomanip>
#include <string>
#include "std_msgs/String.h"
#include <thread>
#include <atomic>
#include <cmath>
#include <tf/tf.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <ctime>
#include <std_msgs/Float64.h>
#include <string>
#include "driver/position.h"
#include "driver/position_quaternion.h"

#define PAI 3.14159
#define radius 0.084           //eld o.86
#define base_width 0.45
#define encoder_ticks_per_rev 262027  //16384  //264  //2620270     //eld   250000

class ruitu_driver
{
public:
    ruitu_driver();
    ~ruitu_driver();

    void ruitu_loop();
    /*
    void receive_left(const std_msgs::Float64::ConstPtr& msg);
    void receive_right(const std_msgs::Float64::ConstPtr& msg);
    */
    void receive(const std_msgs::String::ConstPtr& msg);
    void extract_number_from_string(std::string & ext_str,std::vector<float> & arr);
    bool change_position(driver::position::Request & req, driver::position::Response & res);
    bool change_position_quaternion(driver::position_quaternion::Request & req, 
                                    driver::position_quaternion::Response & res);
    void quaternion_to_EulerAngle(const double x,const double y,const double z,const double w, 
                                  double& roll, double& pitch, double& yaw);
    //bool change_position();

    void calculator();
    void pub();
    void param_to_set();

    std::string receive_string;
    std::vector<float> code;

    bool publish_odom_transform_ = true;

    float left_code;
    float right_code;
    float left_code_last;
    float right_code_last;

    float x_world;
    float y_world;
    float distance_world;
    float angle_world;

    float dleft;
    float dright;
    float dangle;

    float dx_word;
    float dy_word;

//speed
    
    float speed_x;
    float speed_y;
    float speed_angle;

//time
    
    ros::Time now;
    ros::Time last_time;
    float time_pase;

//ros
    ros::NodeHandle nh_;
    /*
    ros::Subscriber sub_encoder_left;
    ros::Subscriber sub_encoder_right;
    */
    ros::Subscriber sub_encoder;

    nav_msgs::Odometry odom_;
    ros::Publisher odom_pub;
    ros::ServiceServer service_change_position;
    ros::ServiceServer service_change_position_quaternion;

//tf 
    geometry_msgs::TransformStamped transformStamped_;
    tf2_ros::TransformBroadcaster br_;
    tf2::Quaternion q;

//param
    std::string base_frame_;
    std::string odom_frame_;
    std::string odom_frame_tf;

private:
    /* data */
};


ruitu_driver::ruitu_driver()
{
    left_code = 0;
    right_code = 0;
    left_code_last = 0;
    right_code_last = 0;
    x_world = 0;
    y_world = 0;
    distance_world = 0;
    angle_world = 0;
    dleft = 0;
    dright = 0;
    dangle = 0;
    dx_word = 0;
    dy_word = 0;
    speed_x = 0;
    speed_y = 0;
    speed_angle = 0;
    receive_string = "";
    code.push_back(10);
    code.push_back(10);
    now = ros::Time::now();
    last_time = ros::Time::now();
}

ruitu_driver::~ruitu_driver()
{

}
void ruitu_driver::ruitu_loop()
{
    /*
    sub_encoder_left = nh_.subscribe<std_msgs::Float64>("left",10,&ruitu_driver::receive_left,this);
    sub_encoder_right = nh_.subscribe<std_msgs::Float64>("right",10,&ruitu_driver::receive_right,this);
    */
    param_to_set();
    sub_encoder = nh_.subscribe<std_msgs::String>("/back_position",1,&ruitu_driver::receive,this);
    odom_pub = nh_.advertise<nav_msgs::Odometry>("odom_raw",10);
    service_change_position = nh_.advertiseService("change_position",&ruitu_driver::change_position,this);
    service_change_position_quaternion = nh_.advertiseService("change_position_quaternion",
                                                              &ruitu_driver::change_position_quaternion,this);
    ros::Rate loop_rate(20);
    while (ros::ok())
    {        
        /* code for loop body */ 
        ros::spinOnce();
        calculator();
        pub();
        loop_rate.sleep();//sleep(1);
    }
    
}

bool ruitu_driver::change_position(driver::position::Request & req, driver::position::Response & res)
{
    try{
        this->x_world = req.new_position_for_x;
        this->y_world = req.new_position_for_y;
        this->angle_world = req.new_position_for_angle_world;
        res.success = true;
    }catch(bool){
        res.success = false;
        return false;
    }
    return true;
}

bool ruitu_driver::change_position_quaternion(driver::position_quaternion::Request & req, 
                                              driver::position_quaternion::Response & res)
{
    try{
        this->x_world = req.new_position_for_x;
        this->y_world = req.new_position_for_y;
        double roll;
        double pitch; 
        double yaw;
        quaternion_to_EulerAngle(req.new_position_for_quaternionx,
                                 req.new_position_for_quaterniony,
                                 req.new_position_for_quaternionz,
                                 req.new_position_for_quaternionw,roll,pitch,yaw);
        

        this->angle_world = yaw;
        res.success = true;
    }catch(bool){
        res.success = false;
        return false;
    }
    return true;
}

void ruitu_driver::quaternion_to_EulerAngle(const double x,const double y,const double z,const double w, 
                                            double& roll, double& pitch, double& yaw)
{
    // roll (x-axis rotation)
    double sinr_cosp = +2.0 * (w * x + y * z);
    double cosr_cosp = +1.0 - 2.0 * (x * x + y * y);
    roll = atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = +2.0 * (w * y - z * x);
    if (fabs(sinp) >= 1)
        pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        pitch = asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = +2.0 * (w * z + x * y);
    double cosy_cosp = +1.0 - 2.0 * (y * y + z * z);
    yaw = atan2(siny_cosp, cosy_cosp);
    //    return yaw;
}

void ruitu_driver::param_to_set()
{
    nh_.param<std::string>("base_frame",base_frame_,std::string("base_footprint"));
    nh_.param<std::string>("odom_frame",odom_frame_,std::string("odom_raw"));
    nh_.param<std::string>("odom_frame",odom_frame_tf,std::string("odom_tf"));

}

/*
void ruitu_driver::receive_left(const std_msgs::Float64::ConstPtr& msg)
{
    this->left_code = msg->data;
    std::cout<<this->left_code<<" "<<msg->data<<std::endl;
    //rostopic pub right std_msgs/Float64 1 1
}

void ruitu_driver::receive_right(const std_msgs::Float64::ConstPtr& msg)
{
    this->right_code = msg->data;
    std::cout<<this->right_code<<" "<<msg->data<<std::endl;
}
*/

void ruitu_driver::receive(const std_msgs::String::ConstPtr& msg)
{
    receive_string = msg->data.c_str();
    std::cout<<"    收到信息    "<<receive_string<<std::endl;
    extract_number_from_string(this->receive_string,this->code);
}

void ruitu_driver::extract_number_from_string(std::string & ext_str,std::vector<float> & arr)
{
    int counter = arr.size();
    for(int m = 0; m<counter; m++)
    {
        arr.pop_back();
    }
    std::cout<<"########################"<<std::endl;
    std::cout<<"    应该为 零    "<<arr.size()<<std::endl;
    int i=0;
	int data = 0;
	bool update = false;//一个数字结束标志位
	while (i<ext_str.size())
	{
		if (ext_str[i] >= '0'&&ext_str[i] <= '9')
		{
			data *= 10;
			data += ext_str[i] - '0';
			update = true;
		}
		else if(update)
		{
			arr.push_back(data);
			data = 0;
			update = false;
		}
		if(update&&i == ext_str.size() - 1)//需对字符串最后的数字进行判断
			arr.push_back(data);
		i++;    
	}
    left_code = arr[1];
    right_code = arr[4];
    std::cout<<"    左车轮    "<<left_code<<std::endl;
    std::cout<<"    右车轮    "<<right_code<<std::endl;
    std::cout<<"########################"<<right_code<<std::endl;
}

void ruitu_driver::calculator()
{    
     std::cout<<"开始计算"<<std::endl;
     std::cout<<"************************************"<<std::endl;
     //time_current = rospy.Time.now()
     //time_elapsed = (time_current - self.time_prev).to_sec()
     //self.time_prev = time_current
     now = ros::Time::now();
     time_pase = (now - last_time).toSec();
     last_time = ros::Time::now();
     std::cout<<"当前时间        "<<time_pase<<std::endl;
     if(this->left_code != 0 && this->left_code_last != 0 && 
        this->right_code !=0 && this->right_code_last !=0)
     {
        this->dleft = (PAI * radius * (left_code - left_code_last))/encoder_ticks_per_rev;  //两次读数前进m数
        this->dright = (PAI * radius * (right_code - right_code_last))/encoder_ticks_per_rev;  //两次读数前进m数
     }

     /*
     std::cout<<left_code<<"  0000000000"<<left_code_last<<"    0000000000000"<<std::endl;
     std::cout<<(left_code - left_code_last)<<"  0000000000000000000dushuchabie"<<std::endl;
     int mmmm = left_code - left_code_last;
     this->dleft = ((3.14 * 0.075 * (left_code - left_code_last))/10000);
     this->dright = ((3.14 * 0.075 * (right_code - right_code_last))/10000);
     */

     this->left_code_last = left_code;  //读数
     this->right_code_last = right_code;  //读数
     this->distance_world = (dleft + dright) / 2;  //中心前进距离
     this->dangle = (dleft - dright)/base_width;  //计算两 y_world +次读数偏转角
     if(this->distance_world != 0)
     {
         dx_word = cos(dangle)*this->distance_world; //累积到总数(base)
         dy_word = -sin(dangle)*this->distance_world; //累积到总数(base)
         x_world +=  dx_word*cos(angle_world) - dy_word*sin(angle_world); //累积到总数(odom)
         y_world +=  dx_word*sin(angle_world) + dy_word*cos(angle_world); //累积到总数(odom)
         //dx*math.cos(self.theta)-dy*math.sin(self.theta)
         //self.y += dx*math.sin(self.theta)+dy*math.cos(self.theta)
     }
     angle_world += dangle;  //累积到总数偏转角
     if(time_pase!=0)
     {
         speed_x = dx_word /time_pase;
         speed_y = dx_word /time_pase;
         speed_angle = dangle / time_pase;
     }
     std::cout<<"叠加x        "<<(dx_word*cos(angle_world) - dy_word*sin(angle_world))<<std::endl;
     std::cout<<"叠加y        "<<(dx_word*sin(angle_world) + dy_word*cos(angle_world))<<std::endl;
     std::cout<<"最终x        "<<"->->->   "<<x_world<<std::endl;
     std::cout<<"最终y        "<<"->->->   "<<y_world<<std::endl;
     std::cout<<"最终angle    "<<angle_world<<std::endl;
     std::cout<<"************************************"<<std::endl;
}

void ruitu_driver::pub()
{   
    std::cout<<"-----------"<<std::endl;
    std::cout<<"已经发布"<<std::endl;
    std::cout<<"-----------"<<std::endl;
    odom_.header.frame_id = odom_frame_;
    odom_.child_frame_id  = base_frame_;
    odom_.header.stamp    = now;
    odom_.pose.pose.position.x = x_world;
    odom_.pose.pose.position.y = y_world;
    odom_.pose.pose.position.z = 0;
    q.setRPY(0,0,angle_world);
    odom_.pose.pose.orientation.x = q.getX();
    odom_.pose.pose.orientation.y = q.getY();
    odom_.pose.pose.orientation.z = q.getZ();
    odom_.pose.pose.orientation.w = q.getW();
    odom_.twist.twist.linear.x = speed_x;
    odom_.twist.twist.linear.y = speed_y;
    odom_.twist.twist.angular.z = speed_angle;
    odom_.twist.covariance = { 1e-9, 0, 0, 0, 0, 0, 
                              0, 1e-3, 1e-9, 0, 0, 0, 
                              0, 0, 1e6, 0, 0, 0,
                              0, 0, 0, 1e6, 0, 0, 
                              0, 0, 0, 0, 1e6, 0, 
                              0, 0, 0, 0, 0, 0.10 };
    odom_.pose.covariance = { 1e-9, 0, 0, 0, 0, 0, 
                              0, 1e-3, 1e-9, 0, 0, 0, 
                              0, 0, 1e6, 0, 0, 0,
                              0, 0, 0, 1e6, 0, 0, 
                              0, 0, 0, 0, 1e6, 0, 
                              0, 0, 0, 0, 0, 1e3 };
    odom_pub.publish(odom_);
    transformStamped_.header.stamp    = now;
    transformStamped_.header.frame_id = odom_frame_;
    transformStamped_.child_frame_id  = base_frame_;
    transformStamped_.transform.translation.x = x_world;
    transformStamped_.transform.translation.y = y_world;
    transformStamped_.transform.translation.z = 0.0;
    transformStamped_.transform.rotation.x = q.x();
    transformStamped_.transform.rotation.y = q.y();
    transformStamped_.transform.rotation.z = q.z();
    transformStamped_.transform.rotation.w = q.w();
    if(publish_odom_transform_)
    {
        br_.sendTransform(transformStamped_);
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "driver_odom");
    ruitu_driver *drive = new ruitu_driver();
    drive->ruitu_loop();
    delete drive;
    return 0;
}